#include "rgb_led.h"

#define RGB_LED_TOTAL 28

#define LED_PORT (PortA)
#define LED_PIN (Pin01)

static void bit0(void)
{
    *(uint16_t *)((uint32_t)(&M4_PORT->POSRA) + 0x10u * LED_PORT) |= LED_PIN;

    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");

    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");

    __ASM("nop");
    __ASM("nop");

    *(uint16_t *)((uint32_t)(&M4_PORT->PODRA) + 0x10u * LED_PORT) &= (uint16_t)(~LED_PIN);

    for (uint8_t i = 0; i < 25; i++)
    {
        __ASM("nop");
    }
}

static void bit1(void)
{
    *(uint16_t *)((uint32_t)(&M4_PORT->POSRA) + 0x10u * LED_PORT) |= LED_PIN;

    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");

    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");

    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");

    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");

    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");

    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");

    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");

    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");

    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");
    __ASM("nop");

    *(uint16_t *)((uint32_t)(&M4_PORT->PODRA) + 0x10u * LED_PORT) &= (uint16_t)(~LED_PIN);
    for (uint8_t i = 0; i < 25; i++)
    {
        __ASM("nop");
    }
}

void rgb_led_init()
{
    stc_port_init_t stcPortInit;
    MEM_ZERO_STRUCT(stcPortInit);
    stcPortInit.enPinMode = Pin_Mode_Out;
    PORT_Init(LED_PORT, LED_PIN, &stcPortInit);
}

void rgb_led_set(uint8_t R, uint8_t G, uint8_t B)
{
    __disable_irq();

    uint8_t data[3] = {G, R, B};
    uint8_t index[24] = {0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,2,2,2,2,2,2,2,2};
    for (uint8_t j = 0; j < RGB_LED_TOTAL; j++)
    {
        for (uint8_t i = 0; i < 24; i++)
        {
            if (data[index[i]] & (0x808080 >> i))
            {
                bit1();
            }
            else
            {
                bit0();
            }
        }
    }
    __enable_irq();
}
